#include "pathengine.h"

void CPathEngine::SetMap(CCTMXTiledMap* map)
{
	_map = map;
}

bool CPathEngine::IsValidPoint(CCPoint point)
{
	auto metaset = _map->layerNamed("metaset");
	auto bglayer = _map->layerNamed("background");

	CCSize layerSize = metaset->getLayerSize();
	CCSize tileSize = metaset->getMapTileSize();

	CCPoint tilePos;
	tilePos.x = int(point.x / tileSize.width);
	tilePos.y = int((layerSize.height * tileSize.height - point.y) / tileSize.height);

	CCSprite* sprite = bglayer->tileAt(tilePos);
	if(NULL == sprite)
		return false;

	short gid = metaset->tileGIDAt(tilePos);
	CCMutableDictionary<string, CCString*>* dict = _map->propertiesForGID(gid);
	if(dict)
	{
		CCString* roadValue = dict->objectForKey("road");
		if(roadValue && roadValue->m_sString == "yes")
		{
			return true;
		}
	}

	return false;
}

void CPathEngine::GetChildNode(CNode &node, vector<CNode> &vtNewNode )
{
	auto metaset = _map->layerNamed("metaset");
	auto bglayer = _map->layerNamed("background");

	CNode newNode;
	for(int i = 0;i < 4; ++i)
	{
		if(i == 0)
		{
			newNode.tilePos.x = node.tilePos.x - 1;
			newNode.tilePos.y = node.tilePos.y;
		}
		else if(i == 1)
		{
			newNode.tilePos.x = node.tilePos.x + 1;
			newNode.tilePos.y = node.tilePos.y;
		}
		else if(i == 2)
		{
			newNode.tilePos.x = node.tilePos.x;
			newNode.tilePos.y = node.tilePos.y - 1;
		}
		else if(i == 3)
		{
			newNode.tilePos.x = node.tilePos.x;
			newNode.tilePos.y = node.tilePos.y + 1;
		}

		if(newNode.tilePos.x >= metaset->getLayerSize().width 
			|| newNode.tilePos.y >= metaset->getLayerSize().height 
			|| newNode.tilePos.x < 0 
			|| newNode.tilePos.y < 0)
			continue;


		newNode.index = CreateNodeIndex(newNode.tilePos);

		short gid = metaset->tileGIDAt(newNode.tilePos);
		CCMutableDictionary<string, CCString*>* dict = _map->propertiesForGID(gid);
		if(dict)
		{
			CCString* roadValue = dict->objectForKey("road");
			if(roadValue && roadValue->m_sString == "yes")
			{
				vtNewNode.push_back(newNode);
			}
		}
	}
}




void CPathEngine::findPath(CCPoint& startPos, CCPoint& goalPos, list<CCPoint> &pathlist)
{
	using namespace std;

	auto metaset = _map->layerNamed("metaset");
	auto bglayer = _map->layerNamed("background");
	CCSize layerSize = metaset->getLayerSize();
	CCSize tileSize = metaset->getMapTileSize();

	typedef multimap<short/*TotalCost*/,short /*parentIndex*/> OPENLIST;
	typedef std::map<short /*parentIndex*/, CNode> OPENNODE;
	typedef std::map<short /*parentIndex*/, CNode> CLOSENODE;

	OPENLIST openList;
	OPENNODE openNode; 
	CLOSENODE closeNode;


	CNode rootNode;
	CCPoint tilePosOfGoal;


	rootNode.tilePos.x = int(startPos.x / tileSize.width);
	rootNode.tilePos.y = int((layerSize.height * tileSize.height - startPos.y) / tileSize.height);
	tilePosOfGoal.x = int(goalPos.x / tileSize.width);
	tilePosOfGoal.y = int((layerSize.height * tileSize.height - goalPos.y) / tileSize.height);


	CCSprite* sprite = bglayer->tileAt(tilePosOfGoal);
	if(NULL == sprite)
		return;

	short gidOfEndPos = CreateNodeIndex(tilePosOfGoal);

	rootNode.costFromStart = 0;
	rootNode.costToGoal = abs(tilePosOfGoal.x - rootNode.tilePos.x) + abs(tilePosOfGoal.y - rootNode.tilePos.y);
	rootNode.parentNode = 0;

	sprite = bglayer->tileAt(rootNode.tilePos);
	if(NULL == sprite)
		return;

	short rootGid = CreateNodeIndex(rootNode.tilePos);
	openList.insert(OPENLIST::value_type(rootNode.costFromStart + rootNode.costToGoal, rootGid));
	openNode.insert(OPENNODE::value_type(rootGid, rootNode));

	while(!openList.empty())
	{
		auto parentIndex = openList.begin()->second;
		auto iter = openNode.find(parentIndex);
		CNode node = iter->second;
		// Pop openlist
		openList.erase(openList.begin());
		openNode.erase(iter);

		if(parentIndex == gidOfEndPos)
		{
			pathlist.push_front(goalPos);
			short pIndex = node.parentNode;
			while(pIndex && pIndex != rootGid)
			{
				CNode* parentNode = NULL;
				auto iter1 = openNode.find(pIndex);
				if(iter1 != openNode.end())
					parentNode = &iter1->second;

				auto iter2 = closeNode.find(pIndex);
				if(iter2 != closeNode.end())
					parentNode = &iter2->second;

				if(parentNode)
				{
					pathlist.push_front(ccp((parentNode->tilePos.x) * tileSize.width + 16, 
						(layerSize.height - parentNode->tilePos.y) * tileSize.height - 16));
					pIndex = parentNode->parentNode;
				}

			}

			return;
		}
		else
		{
			vector<CNode> vtNewNode;
			GetChildNode(node, vtNewNode);

			for(size_t i = 0; i < vtNewNode.size(); ++i)
			{
				CNode newNode = vtNewNode[i];
				auto indexOfNewNode = vtNewNode[i].index;


				newNode.costFromStart = node.costFromStart + 1;

				auto onitr = openNode.find(indexOfNewNode);
				if(onitr != openNode.end())
				{
					if(newNode.costFromStart >= onitr->second.costFromStart)
						continue;
				}

				auto clitr = closeNode.find(indexOfNewNode);
				if(clitr != closeNode.end())
				{
					if(newNode.costFromStart >= clitr->second.costFromStart)
						continue;
				}

				newNode.costToGoal = abs(tilePosOfGoal.x - newNode.tilePos.x) + abs(tilePosOfGoal.y - newNode.tilePos.y);
				newNode.parentNode = parentIndex;

				if(clitr != closeNode.end())
					closeNode.erase(clitr);

				if(onitr != openNode.end())
				{
					short oldCost = onitr->second.costFromStart + onitr->second.costToGoal;
					auto oLitr = openList.equal_range(oldCost);
					for(auto it = oLitr.first; it != oLitr.second; ++it)
					{
						if(it->first == indexOfNewNode)
						{
							openList.erase(it);
							openList.insert(OPENLIST::value_type(newNode.costFromStart + newNode.costToGoal, indexOfNewNode));
							onitr->second = newNode;
						}
					}
				}
				else
				{
					openList.insert(OPENLIST::value_type(newNode.costFromStart + newNode.costToGoal, indexOfNewNode));
					openNode.insert(OPENNODE::value_type(indexOfNewNode, newNode));
				}
			}
		}

		closeNode.insert(CLOSENODE::value_type(parentIndex, node));		
	}
}